//
// Created by hoyin on 2022/3/28.
//

#ifndef FILTER_OPTIMIZER_H
#define FILTER_OPTIMIZER_H

#include "ceres/ceres.h"

using namespace std;
using namespace ceres;

namespace opt {
	struct CURVE_FITTING_COST_TRIGONOMETRIC {
		CURVE_FITTING_COST_TRIGONOMETRIC(double x, double y) : _x(x), _y(y) {}

		template<typename T>
		bool operator() (const T *const awty, T *residual) const {
			residual[0] = T(_y) - (awty[0] * ceres::sin(awty[1] * (T(_x) + awty[2])) + awty[3]);
			return true;
		}

		const double _x, _y;
	};

	class Optimizer {
	private:
//		采样数
		int N = 20;
//		优化参数
		double *params;
//		优化器游标
		int flag = 0;
	public:
		Optimizer(double parameters[], int capacity) : N(capacity) {
			params = parameters;
		}

		void solveTrigonometric(vector<double> x_data, vector<double> y_data) {
			Problem problem;
			Solver::Options options;
			options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
			options.minimizer_progress_to_stdout = false;
			Solver::Summary summary;
			for (int i = 0; i < flag; ++i) {
				problem.AddResidualBlock(
						new AutoDiffCostFunction<CURVE_FITTING_COST_TRIGONOMETRIC, 1, 4> (
								new CURVE_FITTING_COST_TRIGONOMETRIC(x_data[i], y_data[i])
						),
						nullptr,
						params
				);
			}

			Solve(options, &problem, &summary);
		}

		void increaseFlag() {
			flag++;
		}

		void decreaseFlag() {
			flag--;
		}

		int getFlag() const {
			return flag;
		}

		void setCapacity(int capacity) {
			N = capacity;
		}

		int getCapacity() const {
			return N;
		}

		bool readyToOptimize() const {
//			return flag % N == 0 && flag > N;
			return flag > N;
		}
	};
}
#endif //FILTER_OPTIMIZER_H
